package mdp.algorithm.explore.noconnection;

import mdp.algorithm.simulator.*;
import mdp.algorithm.explore.Sensors;
import mdp.algorithm.fastestPath.PathSolver;

import java.awt.Point;
import java.util.Scanner;

import javax.swing.JLabel;

public class Robot {
	
	private int counter = 0;
	private int virtualMap[][];
	private int robotOrientation;
	private int robotMidPointRow, robotMidPointCol; //Robot's current position
	
	private float minimumCoverage = 0.8f;
	
	private boolean goal = false, start;
	
	private String command;
	private Sensors sensors;
	private Stopwatch stopwatch;
	private Scanner in = new Scanner(System.in);
	
	private static final float TIMEOUT = 20;
	
	private static final String C_RARDUINO = "H:", C_ARDUINO = "HP:", C_ASTART = "A:XP", C_ANDROID = "AP:",
								C_ASOLVE="A:XS", C_APOLL = "A:GRID", C_LEFT = "L", C_RIGHT = "L", 
								C_180 = "B", C_ADVANCE = "F", /*C_SOLVE="AS",*/ C_START="X", 
								C_STOP = "S", C_LX = "LX", C_RX = "RX", C_FX = "FX", C_FY = "FY", C_FZ="FZ";
	private static final String WARNING_LABEL = "Warning:\t", COMMAND_RECEIVED_LABEL = "Command Received:\t";
	
	private static final int BITS = 13;
	private static final int MAP_ROW = 22, MAP_COL = 17;
	private static final int NORTH = 0, EAST = 1, SOUTH = 2, WEST = 3;
	private static final int UNKNOWN = 0, EXPLORED = 1, OBSTACLES = 2, ROBOT_POSITION = 3 ;
	private static final int DEFAULT_ROBOT_MIDPOINT_ROW = 11, DEFAULT_ROBOT_MIDPOINT_COL = 8, DEFAULT_ORIENTATION = NORTH;
	
	private Maze maze;
	
	public Robot(Maze maze, JLabel exploreTimeValue, String timeLimit, float minimumCoverage) {
		start = true;
		
		this.maze = maze;
		this.minimumCoverage = minimumCoverage;

		sensors = new Sensors();

		/*BigInteger val = new BigInteger("0000100011111111", 2);
		//Binary to String
		String test = new String(val.toByteArray()); 
		System.out.println(test);
		val = new BigInteger(test.getBytes());
		//String to Binary
		System.out.println(val.toString(2));
		//String to Binary String
		System.out.println(new BigInteger(test.getBytes()).toString(2));
		System.out.println("Size: " + new BigInteger(test.getBytes()).toString(2).length());*/
		
		initRobot(DEFAULT_ROBOT_MIDPOINT_ROW, DEFAULT_ROBOT_MIDPOINT_COL, DEFAULT_ORIENTATION);
		
		System.out.println("Simulated Command Received.\nA:X\n");
		System.out.println("Simulated Command Sent. \nH:X\n");
		
		System.out.println("First Time Polling for Telemetric Information.\n");
		System.out.println("Enter 13bit Binary String for Sensor Data:\n");
		updateTelemetricInformation(in.nextLine());

		int i = 0;
		while(mapCoveredPercentage() < this.minimumCoverage) {
				explore();
				maze.esSetExplorationPathAction();
		}
		stopExplore();
	}
	
	/*****************
	 *   ALGORITHM   *
	 *****************/
	public void explore() {
		if(!goal)
			goal = isRobotAtGoal();	
		if(start) {
			if (robotMidPointRow == 2 || robotMidPointRow == 19 || robotMidPointCol == 2 || robotMidPointCol == 14) {
				
			} else {
				if (counter == 0) {
					if (isFrontClear())
						moveForward();
					else {
						spinLeft();
						counter--;
					}
				} else {
					if (isStartRightClear()) {
						spinRight();
						if (isFrontClear())
							moveForward();
						counter++;
					} else if (isFrontClear()) {
						moveForward();
					} else {
						spinLeft();
						if(isFrontClear()) {
							moveForward();
						}
						counter--;
					}
				}
			}
		}
		else {
			if (isRightClear()) {
				spinRight();
				if(isFrontClear())
					moveForward();
			} else if (isFrontClear())
				moveForward();
			else if (isLeftClear()) {
				spinLeft();
				if (isFrontClear())
					moveForward();
			} else {
				spin180();
				if (isFrontClear()) {
					moveForward();
				}
			}
		}
	}
	
	/* UNTOUCHED */
	private void stopExplore() {
		command = C_ARDUINO+C_STOP;
		System.out.println("Simulated Command Sent: " + command);
		Point start = new Point(maze.getRobotMidPointRow(), maze.getRobotMidPointCol());
		
		if((maze.checkExplored(2,13)|| maze.checkExplored(3,14)) && maze.checkUnexplored(2,14)){
        	PathSolver ps1 = new PathSolver();
        	Point goal1 = new Point(2,14);
    		ps1.moveRobot(ps1.solvePath(start,goal1,maze,maze.getRobotOrientation()),start,goal1,maze,maze.getRobotOrientation());
        }
		
		stopwatch.stopStopwatchTimer();
		
        System.out.println("Waiting for Android to issue FASTEST PATH command.\n");
        System.out.println("Simulated Command Received: " + C_ASOLVE);
		
        //go to start
		Point goal2 = new Point(19,2);
        PathSolver ps2 = new PathSolver();
		ps2.moveRobot(ps2.solvePath(start,goal2,maze,maze.getRobotOrientation()),start,goal2,maze,maze.getRobotOrientation());
		maze.completePath();
		
	}
	/*END UNTOUCH*/
	
	/*********************************************
	 *   DO NOT MAKE ANY CHANGES FROM HERE ON.   *
	 *********************************************/
	/* Robot Memory Access */
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private int mapCoverage() {
		int gridsCovered = 0;
		for (int i = 1; i < MAP_ROW-1; i++)
			for (int j = 1; j < MAP_COL-1; j++) {
				if (virtualMap[i][j] != 0)
					gridsCovered++;
			}
		return gridsCovered;
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private double mapCoveredPercentage() {
		if (mapCoverage() == 0)
			return 0;
		return mapCoverage()/(MAP_ROW*MAP_COL);
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private boolean isRobotAtGoal(){
		if(robotMidPointRow==2 && robotMidPointCol == 14)
			return true;
		return false;
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void printVirtualMap() {
		System.out.println("\n");
		for (int row = 0; row < MAP_ROW; row++) {
			for (int col = 0; col < MAP_COL; col++)
				System.out.print(virtualMap[row][col] + "	");
			System.out.println("\n");
		}
	}
	
	/* Telemetric Manager */
	private String telemetricPolling() {
		String telemetricCommand = "";
		System.out.println("Polling for Telemetric Information. Please enter a 13-bit binary:\n");
		telemetricCommand = in.nextLine();
		return telemetricCommand;
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void updateTelemetricInformation(String syntax) {
		String data;
		int leftSensorReading = 0, rightSensorReading = 0;
		//Detected Obstacles on Left
		data = syntax.substring(0,1);
		leftSensorReading = (data.equals("1") ? 1 : leftSensorReading);
		sensors.setLeftObstacles(0, (data.equals("1") ? true : false));
		data = syntax.substring(1,2);
		leftSensorReading = (data.equals("1") ? 2 : leftSensorReading);
		sensors.setLeftObstacles(1, (data.equals("1") ? true : false));
		data = syntax.substring(2,3);
		leftSensorReading = (data.equals("1") ? 3 : leftSensorReading);
		sensors.setLeftObstacles(2, (data.equals("1") ? true : false));
		data = syntax.substring(3,4);
		leftSensorReading = (data.equals("1") ? 4 : leftSensorReading);
		sensors.setLeftObstacles(3, (data.equals("1") ? true : false));
		data = syntax.substring(4,5);
		leftSensorReading = (data.equals("1") ? 5 : leftSensorReading);
		sensors.setLeftObstacles(4, (data.equals("1") ? true : false));
		
		//Detected Obstacles on Right
		data = syntax.substring(5,6);
		rightSensorReading = (data.equals("1") ? 1 : rightSensorReading);
		sensors.setRightObstacles(0, (data.equals("1") ? true : false));
		data = syntax.substring(6,7);
		rightSensorReading = (data.equals("1") ? 2 : rightSensorReading);
		sensors.setRightObstacles(1, (data.equals("1") ? true : false));
		data = syntax.substring(7,8);
		rightSensorReading = (data.equals("1") ? 3 : rightSensorReading);
		sensors.setRightObstacles(2, (data.equals("1") ? true : false));
		data = syntax.substring(8,9);
		rightSensorReading = (data.equals("1") ? 4 : rightSensorReading);
		sensors.setRightObstacles(3, (data.equals("1") ? true : false));
		data = syntax.substring(9,10);
		rightSensorReading = (data.equals("1") ? 5 : rightSensorReading);
		sensors.setRightObstacles(4, (data.equals("1") ? true : false));
		
		//Detected Obstacles in Front
		data = syntax.substring(10,11);
		System.out.println("Sensor FX " + (data.equals("1") ? "has obstacles.":"has no obstacles."));
		sensors.setFrontObstacles(0, (data.equals("1") ? true : false));
		data = syntax.substring(11,12);
		System.out.println("Sensor FY " + (data.equals("1") ? "has obstacles.":"has no obstacles."));
		sensors.setFrontObstacles(1, (data.equals("1") ? true : false));
		data = syntax.substring(12);
		System.out.println("Sensor FZ " + (data.equals("1") ? "has obstacles.":"has no obstacles."));
		sensors.setFrontObstacles(2, (data.equals("1") ? true : false));
		
		//Update Virtual Map
		virtualMap[sensors.getFxRow()][sensors.getFxCol()] = (sensors.getFrontObstacles()[0] ? OBSTACLES :EXPLORED);
		virtualMap[sensors.getFyRow()][sensors.getFyCol()] = (sensors.getFrontObstacles()[1] ? OBSTACLES :EXPLORED);
		virtualMap[sensors.getFzRow()][sensors.getFzCol()] = (sensors.getFrontObstacles()[2] ? OBSTACLES :EXPLORED);

		
		virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()] = (sensors.getLeftObstacles()[0] ? OBSTACLES : EXPLORED);
		virtualMap[sensors.getSensorRVLocationRow()][sensors.getSensorRVLocationCol()] = (sensors.getRightObstacles()[0] ? OBSTACLES : EXPLORED);
		
		if (sensors.getSensorLWLocationRow() < MAP_ROW && sensors.getSensorLWLocationRow() >= 0 && sensors.getSensorLWLocationCol() < MAP_COL && sensors.getSensorLWLocationCol() >= 0)
			virtualMap[sensors.getSensorLWLocationRow()][sensors.getSensorLWLocationCol()] = (leftSensorReading == 2 ? OBSTACLES : (leftSensorReading < 2 && leftSensorReading > 0 ? UNKNOWN : EXPLORED));
		
		if (sensors.getSensorLXLocationRow() < MAP_ROW && sensors.getSensorLXLocationRow() >= 0 && sensors.getSensorLXLocationCol() < MAP_COL && sensors.getSensorLXLocationCol() >= 0)
			virtualMap[sensors.getSensorLXLocationRow()][sensors.getSensorLXLocationCol()] = (leftSensorReading == 3 ? OBSTACLES : (leftSensorReading < 3 && leftSensorReading > 0 ? UNKNOWN : EXPLORED));
		
		if (sensors.getSensorLYLocationRow() < MAP_ROW && sensors.getSensorLYLocationRow() >= 0 && sensors.getSensorLYLocationCol() < MAP_COL && sensors.getSensorLYLocationCol() >= 0)
			virtualMap[sensors.getSensorLYLocationRow()][sensors.getSensorLYLocationCol()] = (leftSensorReading == 4 ? OBSTACLES : (leftSensorReading < 4 && leftSensorReading > 0 ? UNKNOWN : EXPLORED));
		
		if (sensors.getSensorLZLocationRow() < MAP_ROW && sensors.getSensorLZLocationRow() >= 0 && sensors.getSensorLZLocationCol() < MAP_COL && sensors.getSensorLZLocationCol() >= 0)
			virtualMap[sensors.getSensorLZLocationRow()][sensors.getSensorLZLocationCol()] = (leftSensorReading == 5 ? OBSTACLES : (leftSensorReading < 5 && leftSensorReading > 0 ? UNKNOWN : EXPLORED));
		
		if (sensors.getSensorRWLocationRow() < MAP_ROW && sensors.getSensorRWLocationRow() >= 0 && sensors.getSensorRWLocationCol() < MAP_COL && sensors.getSensorRWLocationCol() >= 0)
			virtualMap[sensors.getSensorRWLocationRow()][sensors.getSensorRWLocationCol()] = (rightSensorReading == 2 ? OBSTACLES : (rightSensorReading < 2 && rightSensorReading > 0 ? UNKNOWN : EXPLORED));
		
		if (sensors.getSensorRXLocationRow() < MAP_ROW && sensors.getSensorRXLocationRow() >= 0 && sensors.getSensorRXLocationCol() < MAP_COL && sensors.getSensorRXLocationCol() >= 0)
			virtualMap[sensors.getSensorRXLocationRow()][sensors.getSensorRXLocationCol()] = (rightSensorReading == 3 ? OBSTACLES : (rightSensorReading < 3 && rightSensorReading > 0 ? UNKNOWN : EXPLORED));
		
		if (sensors.getSensorRYLocationRow() < MAP_ROW && sensors.getSensorRYLocationRow() >= 0 && sensors.getSensorRYLocationCol() < MAP_COL && sensors.getSensorRYLocationCol() >= 0)
			virtualMap[sensors.getSensorRYLocationRow()][sensors.getSensorRYLocationCol()] = (rightSensorReading == 4 ? OBSTACLES : (rightSensorReading < 4 && rightSensorReading > 0 ? UNKNOWN : EXPLORED));
		
		if (sensors.getSensorRZLocationRow() < MAP_ROW && sensors.getSensorRZLocationRow() >= 0 && sensors.getSensorRZLocationCol() < MAP_COL && sensors.getSensorRZLocationCol() >= 0)
			virtualMap[sensors.getSensorRZLocationRow()][sensors.getSensorRZLocationCol()] = (rightSensorReading == 5 ? OBSTACLES : (rightSensorReading < 5 && rightSensorReading > 0 ? UNKNOWN : EXPLORED));
		
		printVirtualMap();
		maze.plotMap(sensors);
		System.out.println("Plotted on Map");
	}
	
	/* Robot's Action */
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void moveForward() {
		switch(robotOrientation) {
		case NORTH:
			robotMidPointRow--;
			if (robotMidPointRow < MAP_ROW-2 && robotMidPointRow > 1 && robotMidPointCol < MAP_COL-2 && robotMidPointCol > 1) {
				//Set Trail
				virtualMap[robotMidPointRow+2][robotMidPointCol-1] = EXPLORED;
				virtualMap[robotMidPointRow+2][robotMidPointCol] = EXPLORED;
				virtualMap[robotMidPointRow+2][robotMidPointCol+1] = EXPLORED;
				
				//Set Next Grid
				virtualMap[robotMidPointRow-1][robotMidPointCol-1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow-1][robotMidPointCol] = ROBOT_POSITION;
				virtualMap[robotMidPointRow-1][robotMidPointCol+1] = ROBOT_POSITION;
			}
			break;
		case EAST:
			robotMidPointCol++;
			if (robotMidPointRow < MAP_ROW-2 && robotMidPointRow > 1 && robotMidPointCol < MAP_COL-2 && robotMidPointCol > 1) {
				//Set Trail
				virtualMap[robotMidPointRow-1][robotMidPointCol-2] = EXPLORED;
				virtualMap[robotMidPointRow][robotMidPointCol-2] = EXPLORED;
				virtualMap[robotMidPointRow+1][robotMidPointCol-2] = EXPLORED;
				
				//Set Next Grid
				virtualMap[robotMidPointRow-1][robotMidPointCol+1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow][robotMidPointCol+1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow+1][robotMidPointCol+1] = ROBOT_POSITION; 
			}
			break;
		case SOUTH:
			robotMidPointRow++;
			if (robotMidPointRow < MAP_ROW-2 && robotMidPointRow > 1 && robotMidPointCol < MAP_COL-2 && robotMidPointCol > 1) {
				//Set Trail
				virtualMap[robotMidPointRow-2][robotMidPointCol-1] = EXPLORED;
				virtualMap[robotMidPointRow-2][robotMidPointCol] = EXPLORED;
				virtualMap[robotMidPointRow-2][robotMidPointCol+1] = EXPLORED;
				
				//Set Next Grid
				virtualMap[robotMidPointRow+1][robotMidPointCol-1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow+1][robotMidPointCol] = ROBOT_POSITION;
				virtualMap[robotMidPointRow+1][robotMidPointCol+1] = ROBOT_POSITION;
			}
			break;
		case WEST:
			robotMidPointCol--;
			if (robotMidPointRow < MAP_ROW-2 && robotMidPointRow > 1 && robotMidPointCol < MAP_COL-2 && robotMidPointCol > 1) {
				//Set Trail
				virtualMap[robotMidPointRow-1][robotMidPointCol+2] = EXPLORED;
				virtualMap[robotMidPointRow][robotMidPointCol+2] = EXPLORED;
				virtualMap[robotMidPointRow+1][robotMidPointCol+2] = EXPLORED;
				
				//Set Next Grid
				virtualMap[robotMidPointRow-1][robotMidPointCol-1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow][robotMidPointCol-1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow+1][robotMidPointCol-1] = ROBOT_POSITION;
			}
			break;
		}
		if (robotMidPointRow < MAP_ROW-2 && robotMidPointRow > 1 && robotMidPointCol < MAP_COL-2 && robotMidPointCol > 1) {
			maze.moveForwardAction();
			initSensorLocations();
			command = C_ARDUINO + C_ADVANCE;
			System.out.println("Simulated Command Sent: " + command);
			updateTelemetricInformation(telemetricPolling());
		} else {
			switch(robotOrientation) {
			case NORTH: 
				robotMidPointRow++;
				break;
			case SOUTH:
				robotMidPointRow--;
				break;
			case EAST:
				robotMidPointCol--;
				break;
			case WEST:
				robotMidPointCol++;
				break;
			}
			System.out.println("Warning: Robot is not synchronised with the algorithm.");
		}
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void spinLeft() {
		maze.spinLeftAction();
		robotOrientation = (robotOrientation + 3) % 4;
		initSensorLocations();
		command = C_ARDUINO + C_LEFT;
		System.out.println("Simulated Command Sent:" + command);
		System.out.println("Current Orientation" + (robotOrientation == NORTH? " NORTH.\n":(robotOrientation == SOUTH? " SOUTH.\n":(robotOrientation == EAST? " EAST.\n":" WEST.\n"))));
		updateTelemetricInformation(telemetricPolling());
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void spinRight() {
		maze.spinRightAction();
		robotOrientation = (robotOrientation + 1) % 4;
		initSensorLocations();
		command = C_ARDUINO + C_RIGHT;
		System.out.println("Simulated Command Sent:" + C_ARDUINO + C_180);
		System.out.println("Current Orientation" + (robotOrientation == NORTH? " NORTH.\n":(robotOrientation == SOUTH? " SOUTH.\n":(robotOrientation == EAST? " EAST.\n":" WEST.\n"))));
		updateTelemetricInformation(telemetricPolling());
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void spin180() {
		maze.spinRightAction();
		maze.spinRightAction();
		System.out.println("Current Orientation" + (robotOrientation == NORTH? " NORTH.\n":(robotOrientation == SOUTH? " SOUTH.\n":(robotOrientation == EAST? " EAST.\n":" WEST.\n"))));
		robotOrientation = (robotOrientation + 2) % 4;
		initSensorLocations();
		command = C_ARDUINO + C_180;
		System.out.println("Simulated Command Sent:" + command);
		System.out.println("Current Orientation" + (robotOrientation == NORTH? " NORTH.\n":(robotOrientation == SOUTH? " SOUTH.\n":(robotOrientation == EAST? " EAST.\n":" WEST.\n"))));
		updateTelemetricInformation(telemetricPolling());
	}
	
	/* Check Surrounding*/
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private boolean isFrontClear() {
		if (virtualMap[sensors.getFxRow()][sensors.getFxRow()] != OBSTACLES && 
				virtualMap[sensors.getFyRow()][sensors.getFyCol()] != OBSTACLES &&
				virtualMap[sensors.getFzRow()][sensors.getFzCol()] != OBSTACLES)
			return true;
		return false;
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private boolean isLeftClear() {
		switch(robotOrientation) {
		case NORTH:
			if (virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorLVLocationRow()+1][sensors.getSensorLVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorLVLocationRow()+2][sensors.getSensorLVLocationCol()] != OBSTACLES &&
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()] == UNKNOWN && 
					virtualMap[sensors.getSensorLVLocationRow()+1][sensors.getSensorLVLocationCol()] == UNKNOWN &&
					virtualMap[sensors.getSensorLVLocationRow()+2][sensors.getSensorLVLocationCol()] == UNKNOWN)
				return true;
			break;
		case EAST:
			if (virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()-1] != OBSTACLES &&
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()-2] != OBSTACLES && 
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()-1] == UNKNOWN &&
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()] == UNKNOWN && 
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()+1] == UNKNOWN)
				return true;
			break;
		case SOUTH:
			if (virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorLVLocationRow()-1][sensors.getSensorLVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorLVLocationRow()-2][sensors.getSensorLVLocationCol()] != OBSTACLES &&
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()] == UNKNOWN && 
					virtualMap[sensors.getSensorLVLocationRow()-1][sensors.getSensorLVLocationCol()] == UNKNOWN && 
					virtualMap[sensors.getSensorLVLocationRow()-2][sensors.getSensorLVLocationCol()] == UNKNOWN)
				return true;
			break;
		case WEST:
			if (virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()+1] != OBSTACLES &&
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()+2] != OBSTACLES &&
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()] == UNKNOWN && 
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()+1] == UNKNOWN && 
					virtualMap[sensors.getSensorLVLocationRow()][sensors.getSensorLVLocationCol()+2] == UNKNOWN)
				return true;
			break;
		}
		return false;
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private boolean isRightClear() {
		switch(robotOrientation){
		case 0: //NORTH
			if(virtualMap[robotMidPointRow-1][robotMidPointCol+2]!=OBSTACLES && virtualMap[robotMidPointRow][robotMidPointCol+2]!=OBSTACLES && virtualMap[robotMidPointRow+1][robotMidPointCol+2]!=OBSTACLES)
				return true;
			break;
		case 1: //EAST
			if(virtualMap[robotMidPointRow+2][robotMidPointCol+1]!=OBSTACLES && virtualMap[robotMidPointRow+2][robotMidPointCol]!=OBSTACLES && virtualMap[robotMidPointRow+2][robotMidPointCol-1]!=OBSTACLES)
				return true;
			break;
		case 2: //SOUTH
			if(virtualMap[robotMidPointRow+1][robotMidPointCol-2]!=OBSTACLES && virtualMap[robotMidPointRow][robotMidPointCol-2]!=OBSTACLES && virtualMap[robotMidPointRow-1][robotMidPointCol-2]!=OBSTACLES)
				return true;
			break;
		case 3: //WEST
			if(virtualMap[robotMidPointRow-2][robotMidPointCol-1]!=OBSTACLES && virtualMap[robotMidPointRow-2][robotMidPointCol]!=OBSTACLES && virtualMap[robotMidPointRow-2][robotMidPointCol+1]!=OBSTACLES)
				return true;
			break;
		}
		return false;
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private boolean isStartRightClear() {
		switch(robotOrientation) {
		case NORTH:
			if (virtualMap[sensors.getSensorRVLocationRow()][sensors.getSensorRVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorRVLocationRow()+1][sensors.getSensorRVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorRVLocationRow()+2][sensors.getSensorRVLocationCol()] != OBSTACLES)
				return true;
			break;
		case EAST:
			if (virtualMap[sensors.getSensorRVLocationRow()][sensors.getSensorRVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorRVLocationRow()][sensors.getSensorRVLocationCol()-1] != OBSTACLES &&
					virtualMap[sensors.getSensorRVLocationRow()][sensors.getSensorRVLocationCol()-2] != OBSTACLES)
				return true;
			break;
		case SOUTH:
			if (virtualMap[sensors.getSensorRVLocationRow()][sensors.getSensorRVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorRVLocationRow()-1][sensors.getSensorRVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorRVLocationRow()-2][sensors.getSensorRVLocationCol()] != OBSTACLES)
				return true;
			break;
		case WEST:
			if (virtualMap[sensors.getSensorRVLocationRow()][sensors.getSensorRVLocationCol()] != OBSTACLES && 
					virtualMap[sensors.getSensorRVLocationRow()][sensors.getSensorRVLocationCol()+1] != OBSTACLES &&
					virtualMap[sensors.getSensorRVLocationRow()][sensors.getSensorRVLocationCol()+2] != OBSTACLES)
				return true;
			break;
		}
		return false;
	}	
	/* Loading Robot's Component */
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	public void initRobot(int startRow, int startCol, int robotOrientation) {
		maze.setRobotMidPoint(startRow, startCol);
		robotMidPointRow = startRow;//maze.getRobotMidPointRow();
		robotMidPointCol = startCol;//maze.getRobotMidPointCol();
		this.robotOrientation = robotOrientation;
		maze.setRobotOrientation(robotOrientation);
		maze.setRobotMidPoint(startRow, startCol);
		initSensorLocations();
		System.out.println("Sensors Activated.");
		
		initVirtualMap();
		System.out.println("Virtual Map Initialised.");
		
		initStopwatch();
		System.out.println("Stopwatch Initialised.");
		
		initRaspberryConnection();
		System.out.println("Connection with Raspberry Established.");
		System.out.println("---------- All Systems Ready to be Deployed. ----------");
		
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void initVirtualMap() {
		virtualMap = new int[MAP_ROW][MAP_COL];
		
		for (int row = 0; row < MAP_ROW; row++)
			for (int col = 0; col < MAP_COL; col++)
				virtualMap[row][col] = UNKNOWN;
		
		virtualMap[robotMidPointRow-1][robotMidPointCol-1] = ROBOT_POSITION;
		virtualMap[robotMidPointRow-1][robotMidPointCol] = ROBOT_POSITION;
		virtualMap[robotMidPointRow-1][robotMidPointCol+1] = ROBOT_POSITION;
		
		virtualMap[robotMidPointRow][robotMidPointCol-1] = ROBOT_POSITION;
		virtualMap[robotMidPointRow][robotMidPointCol] = ROBOT_POSITION;
		virtualMap[robotMidPointRow][robotMidPointCol+1] = ROBOT_POSITION;
		
		virtualMap[robotMidPointRow+1][robotMidPointCol-1] = ROBOT_POSITION;
		virtualMap[robotMidPointRow+1][robotMidPointCol] = ROBOT_POSITION;
		virtualMap[robotMidPointRow+1][robotMidPointCol+1] = ROBOT_POSITION;
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void initSensorLocations() {
		int row, col;
		//System.out.println("Robot's Current Orientation: " + (robotOrientation == NORTH ? "NORTH" : (robotOrientation == SOUTH ? "SOUTH" : (robotOrientation == EAST ? "EAST" : "WEST"))));
		switch(robotOrientation) {
			case NORTH:	
					row = robotMidPointRow - 2;
					col = robotMidPointCol;
					sensors.setFrontSensors(row, col-1, row, col, row, col+1);
					row = robotMidPointRow - 1;
					col = robotMidPointCol - 2;
					sensors.setLeftSensors(row, col, row, col - 1, row, col - 2, row, col - 3,row, col - 4);
					col = robotMidPointCol + 2;
					sensors.setRightSensors(row, col, row, col + 1, row, col + 2, row, col + 3, row, col + 4);
					break;
			case EAST:	
					row = robotMidPointRow;
					col = robotMidPointCol + 2;
					sensors.setFrontSensors(row-1, col, row, col, row+1, col);
					row = robotMidPointRow - 2;
					col = robotMidPointCol + 1;
					sensors.setLeftSensors(row, col, row - 1, col, row - 2, col, row - 3, col, row - 4, col);
					row = robotMidPointRow + 2;
					sensors.setRightSensors(row, col, row + 1, col, row + 2, col, row + 3, col, row + 4, col);
					break;
			case SOUTH:	
					row = robotMidPointRow + 2;
					col = robotMidPointCol;
					sensors.setFrontSensors(row, col+1, row, col, row, col-1);
					row = robotMidPointRow + 1;
					col = robotMidPointCol + 2;
					sensors.setLeftSensors(row, col, row, col + 1, row, col + 2, row, col + 3, row, col + 4);
					col = robotMidPointCol - 2;
					sensors.setRightSensors(row, col, row, col - 1, row, col - 2, row, col - 3, row, col - 4);
					break;
			case WEST:
					col = robotMidPointCol - 2;
					row = robotMidPointRow;
					sensors.setFrontSensors(row+1, col, row, col, row-1, col);
					col = robotMidPointCol-1;
					row = robotMidPointRow + 2;
					sensors.setLeftSensors(row, col, row + 1, col, row + 2, col, row + 3, col, row + 4, col);
					row = robotMidPointRow - 2;
					sensors.setRightSensors(row,col, row - 1, col, row - 2, col, row - 3, col, row - 4, col);
					break;
		}
		System.out.println("Updated FX: " + sensors.getFxRow() + ", " + sensors.getFxCol());
		System.out.println("Updated FY: " + sensors.getFyRow() + ", " + sensors.getFyCol());
		System.out.println("Updated FZ: " + sensors.getFzRow() + ", " + sensors.getFzCol());
		System.out.println("Sensory Locations Updated.");
	}	
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void initStopwatch() {
		stopwatch = new Stopwatch();
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void initRaspberryConnection() {
		//rpiInterface = new RaspberryController(true);
	}
	
}
